import math
import time
from socket import *
import numpy as np

from render.TacviewClient import TacviewClient
import pandas as pd
import atexit

'''
These functions are used for absolute lat and lon
port and my_id are according to example in https://www.tacview.net/documentation/acmi/en/

NOTE:
1. Download Tacview
2. Run several episodes and save the trajectories
3. Open the application, start listening
4. Call the `data_replay` as shown in __main__
Custom config:
- PORT, default=21567
- password, default='f16sim'
'''


class TacviewRender:
    def __init__(self):
        self._tempsims = {}
        self._jsbsims = {}
        self.time_interval = 0.001
        self.current_step = 0
        self._create_records = False
        self._create_realtime = False
        self.tacview_client = None
        self.record_data = pd.DataFrame()
        atexit.register(self.saveall)

    def add_sim(self, sim):

        self._jsbsims[sim.id] = sim

    def linkTacview(self):
        self.tacview_client = TacviewClient()
        self.tacview_client.init_tcp()

    # def lat_lon_to_xyz(self, longitude, latitude, altitude):
    #     R = 6371000  # 地球半径
    #     height = altitude * 0.3048
    #     lat_rad = math.radians(latitude)
    #     lon_rad = math.radians(longitude)
    #     x1 = (R + height) * math.cos(lat_rad) * math.cos(lon_rad)
    #     y1 = (R + height) * math.cos(lat_rad) * math.sin(lon_rad)
    #     # z1 = (R + height) * math.sin(lat_rad) - 6371000
    #     z1=height
    #     return x1, y1, z1
    def lat_lon_to_xyz(self, longitude, latitude, altitude):
        R = 6371000  # 地球半径（米）

        # 将角度转换为弧度
        lat_rad = math.radians(latitude)
        lon_rad = math.radians(longitude)

        # 计算地面坐标
        x = R * lon_rad * math.cos(lat_rad)
        y = R * lat_rad
        z = altitude
        return x, y, z

    def calculate_distance(self, x1, y1, z1, x2, y2, z2):
        return math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1))

    # 示例数据

    def render(self, mode="txt", filepath="./render.txt"):
        self.current_step = self.current_step + 1
        if mode == "txt":
            if not self._create_records:
                with open(filepath, mode='w', encoding='utf-8-sig') as f:
                    f.write("FileType=text/acmi/tacview\n")
                    f.write("FileVersion=2.1\n")
                    f.write("0,ReferenceTime=2020-04-01T00:00:00Z\n")
                self._create_records = True
            with open(filepath, mode='a', encoding='utf-8-sig') as f:
                timestamp = self.current_step * self.time_interval
                f.write(f"#{timestamp:.2f}\n")
                for sim in self._jsbsims.values():
                    log_msg = sim.log()
                    if log_msg is not None:
                        f.write(log_msg + "\n")
                for sim in self._tempsims.values():
                    log_msg = sim.log()
                    if log_msg is not None:
                        f.write(log_msg + "\n")
        # TODO: real time rendering [Use FlightGear, etc.]
        elif mode == "realtime":
            if not self._create_realtime:
                self.linkTacview()
                self._create_realtime = True
            else:
                i = 0;
                for sim in self._jsbsims.values():
                    i += 1
                    timestamp = self.current_step * self.time_interval
                    self.tacview_client.send(timestamp, sim.get_state(), sim.uid, color=sim.color)
        elif mode == "pandas":
            data = {}
            red_position = None
            blue_position = None
            for sim in self._jsbsims.values():
                if sim.color == "Red":
                    state = sim.get_state()
                    Gx, Gy, Gz = self.lat_lon_to_xyz(state[0], state[1], state[2])
                    red_position = (Gx, Gy, Gz)  # 保存红色模拟器的位置
                    data.update({"red_lon": [state[0]],
                                 "red_lat": [state[1]],
                                 "red_alt": [state[2]],
                                 "red_roll": [state[3]],
                                 "red_pitch": [state[4]],
                                 "red_yaw": [state[5]],
                                 "red_x": [Gx],
                                 "red_y": [Gy],
                                 "red_z": [Gz]})

                if sim.color == "Blue":
                    state = sim.get_state()
                    Gx, Gy, Gz = self.lat_lon_to_xyz(state[0], state[1], state[2])
                    blue_position = (Gx, Gy, Gz)  # 保存蓝色模拟器的位置
                    data.update({"blue_lon": [state[0]],
                                 "blue_lat": [state[1]],
                                 "blue_alt": [state[2]],
                                 "blue_roll": [state[3]],
                                 "blue_pitch": [state[4]],
                                 "blue_yaw": [state[5]],
                                 "blue_x": [Gx],
                                 "blue_y": [Gy],
                                 "blue_z": [Gz]
                                 })
            if red_position and blue_position:
                distance = self.calculate_distance(red_position[0], red_position[1], red_position[2],
                                                   blue_position[0], blue_position[1], blue_position[2])
                data["distance"] = [distance]  # 将距离添加到数据字典
            new_rows = pd.DataFrame(data)
            self.record_data = pd.concat([self.record_data, new_rows], ignore_index=True)

        else:
            raise NotImplementedError

    def saveall(self):
        self.record_data.to_csv("render.csv")
